#include <Arduino.h>
#include "Wire.h"
#include "GrapeBit.h"
#include "esp32-hal.h"

#define BUZZER_CHANNEL 0
#define NR_OF_LEDS   NUM_RGB*3
#define NR_OF_ALL_BITS 24*NR_OF_LEDS

rmt_data_t led_data[NR_OF_ALL_BITS];
rmt_obj_t* rmt_send = NULL;

const int noteMap[] = {440, 494, 262, 294, 330, 349, 392};

void iicWriteReg(uint8_t addr, uint8_t reg, uint8_t val) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}

uint8_t iicReadReg(uint8_t addr, uint8_t reg) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(addr, (uint8_t)1);
  uint8_t val = Wire.read();
  return val;
}

int parseDigit(){

}

GrapeBit::GrapeBit() {
  Wire.begin(PIN_SDA, PIN_SCL);
  initDA213();

  pinMode(PIN_M1, OUTPUT);
  pinMode(PIN_M2, OUTPUT);
  pinMode(BTNA, INPUT_PULLUP);
  pinMode(BTNB, INPUT_PULLUP);

  analogWrite(PIN_M1, 0);
  analogWrite(PIN_M2, 0);

  // neopixel
  rmt_send = rmtInit(PIN_NEOPIX, RMT_TX_MODE, RMT_MEM_64);
  float realTick = rmtSetTick(rmt_send, 100);

  // buzzer
  uint32_t freq = ledcSetup(BUZZER_CHANNEL, 250, 8);
  ledcAttachPin(PIN_BUZZ, BUZZER_CHANNEL);
  ledcWrite(BUZZER_CHANNEL, 0);
}

int GrapeBit::initDA213() {
  iicWriteReg(DA213ADDR, 0x7f, 0x83);
  iicWriteReg(DA213ADDR, 0x7f, 0x69);
  iicWriteReg(DA213ADDR, 0x7f, 0xbd);
  uint8_t val = iicReadReg(DA213ADDR, 0x8e);
  if (val == 0){
    iicWriteReg(DA213ADDR, 0x8e, 0x50);
  }
  iicWriteReg(DA213ADDR, 0x0f, 0x40);
  iicWriteReg(DA213ADDR, 0x20, 0x00);
  iicWriteReg(DA213ADDR, 0x11, 0x34);
  iicWriteReg(DA213ADDR, 0x10, 0x07);
  iicWriteReg(DA213ADDR, 0x1a, 0x04);
  iicWriteReg(DA213ADDR, 0x15, 0x04);

  return 0;
}

void GrapeBit::updateDA213() {
  uint8_t buff[6];
  Wire.beginTransmission(DA213ADDR);
  Wire.write(0x02);
  Wire.endTransmission(false);
  Wire.requestFrom(DA213ADDR, (uint8_t)6);

  for (uint8_t i=0;i<6;i++) {
    buff[i] = Wire.read();
  }
  
  _x =  (int16_t)(buff[0] | buff[1] << 8) / 16;
  _y = -(int16_t)(buff[2] | buff[3] << 8) / 16;
  _z =  (int16_t)(buff[4] | buff[5] << 8) / 16;
}

float GrapeBit::accX() {
  updateDA213();
  return _x/1024.0;
}

float GrapeBit::accY() {
  updateDA213();
  return _y/1024.0;
}

float GrapeBit::accZ() {
  updateDA213();
  return _z/1024.0;
}

void GrapeBit::setRGB(uint8_t index, uint8_t r, uint8_t g, uint8_t b){
  int color[] = {g, r, b}; // GRB
  int i = index*3*8;
  for(int col=0; col<3; col++ ){
    for(int bit=0; bit<8; bit++){
      if((color[col] & (1<<(7-bit)))){
        // HIGH bit
        led_data[i].level0 = 1; // T1H
        led_data[i].duration0 = 8; // 0.8us
        led_data[i].level1 = 0; // T1L
        led_data[i].duration1 = 4; // 0.4us
      }else{
        // LOW bit
        led_data[i].level0 = 1; // T0H
        led_data[i].duration0 = 4; // 0.4us
        led_data[i].level1 = 0; // T0L
        led_data[i].duration1 = 8; // 0.8us
      }
      i++;
    }
  }
  rmtWrite(rmt_send, led_data, NR_OF_ALL_BITS);
  delay(20);
}

void GrapeBit::motorSet(uint8_t m1, uint8_t m2){
  analogWrite(PIN_M1, m1);
  analogWrite(PIN_M2, m2);
}

void GrapeBit::motorStop(){
  analogWrite(PIN_M1, 0);
  analogWrite(PIN_M2, 0);
}

bool GrapeBit::buttonA(){
  return digitalRead(BTNA) == 0;
}

bool GrapeBit::buttonB(){
  return digitalRead(BTNB) == 0;
}

void GrapeBit::buzz(int freq, int duration){
  ledcWriteTone(BUZZER_CHANNEL, freq);
  delay(duration);
  ledcWriteTone(BUZZER_CHANNEL, 0);
}

void GrapeBit::buzz(int freq, int duration, int delayms){
  buzz(freq, duration);
  delay(delayms);
}

void GrapeBit::playNote(int note, int clap){
  int freq = 440.0f*pow(2, float(note-69)/12);
  int duration = clap * 125;
  // Serial.printf("freq %d, duration %d\r\n", freq, duration);
  buzz(freq, duration);
}

void GrapeBit::playMusic(const char * notes){
  int freq;
  int len = strlen(notes);
  int octave = 4;
  int duration = 500;
  for(int i=0;i<len;i++){
    if(notes[i]>='a' && notes[i]<='g'){
      freq = noteMap[notes[i]-'a'];
      //Serial.println("freq:"+String(freq));
    }else if(notes[i]=='r'){
      freq = 0;
    }else if(notes[i]>='2' && notes[i]<='6'){
      octave = notes[i] - '0';
      //Serial.println("octave:"+String(octave));
    }else if(notes[i]==':'){
      i++;
      duration = (notes[i] - '0')*125;
      //Serial.println("duration:"+String(duration));
    }else if(notes[i]==' '){ // play until we meet a space
      freq *= pow(2, octave-4);
      //Serial.println("play:"+String(freq)+", "+String(duration)+", "+String(octave));
      buzz(freq, duration, duration);
    }
  }
}

